﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace DynaPrecision.Numerical.Interpolator
{
	public class CubicInterpolator<Tx,Ty>:IInterpolator<Tx,Ty>
	{
		#region data
		private List<Tx> XData = new List<Tx>();
		private List<Ty> YData = new List<Ty>();
		private List<Ty> _y2 = new List<Ty>();

		/// <summary>
		/// Tangent at left bound
		/// </summary>
		public Ty yp1
		{
			get;
			set;
		}
		/// <summary>
		/// Tangent at right bound
		/// </summary>
		public Ty ypn
		{
			get;
			set;
		}
		private Tx h, a, b, tempH;
		private int n, klo, khi;
		#endregion

		private void _initiate()
		{
			n = XData.Count;
			if (n == 0)
			{
				return;
			}
			int i, k;
			Tx p, qn, sig, un;
			Ty[] u = new Ty[XData.Count];
			_y2.Add((dynamic)0);
			_y2[0] = u[0] = (dynamic)0;
			for (i = 1; i < n - 1; i++)
			{
				sig = (XData[i] - (dynamic)XData[i - 1]) / (XData[i + 1] - (dynamic)XData[i - 1]);
				p = sig * (dynamic)_y2[i - 1] + 2.0;
				_y2.Add(((dynamic)sig - 1.0) / p);
				u[i] = ((dynamic)YData[i + 1] - YData[i]) / ((dynamic)XData[i + 1] - XData[i]) - ((dynamic)YData[i] - YData[i - 1]) / ((dynamic)XData[i] - XData[i - 1]);
				u[i] = (6.0 * (dynamic)u[i] / (XData[i + 1] - (dynamic)XData[i - 1]) - sig * (dynamic)u[i - 1]) / p;
			}
			qn = un = (dynamic)0.0;
			_y2.Add((un - qn * (dynamic)u[n - 2]) / (qn * (dynamic)_y2[n - 2] + 1.0));
			for (k = n - 2; k >= 1; k--)
			{
				_y2[k] = _y2[k] * (dynamic)_y2[k + 1] + u[k];
			}
			klo = khi = 1;
			return;
		}
		protected void _positionIndex(Tx xa)
		{
			if (XData[klo] <= (dynamic)xa && XData[khi] >= (dynamic)xa && klo != khi && khi - klo <= 1)
			{
				a = (XData[khi] - (dynamic)xa) * tempH;
				b = (xa - (dynamic)XData[klo]) * tempH;
				return;
			}
			int k = 0;
			klo = 0;
			khi = n - 1;
			while (khi - klo > 1)
			{
				k = (khi + klo) >> 1;
				if ((dynamic)XData[k] > xa)
					khi = k;
				else
					klo = k;
			}
			h = (XData[khi] - (dynamic)XData[klo]);
			tempH = 1.0 / (dynamic)h;
			a = (XData[khi] - (dynamic)xa) * tempH;
			b = (xa - (dynamic)XData[klo]) * tempH;
			return;
		}
		public Ty Evaluate(Tx xa)
		{
			if (xa < (dynamic)XData[0])
				return (xa - (dynamic)XData[0]) * yp1 + YData[0];
			if (xa > (dynamic)XData[n - 1])
				return (xa - (dynamic)XData[n - 1]) * ypn + YData[n - 1];
			_positionIndex(xa);
			return a * (dynamic)YData[klo] + b * (dynamic)YData[khi] + ((a * (dynamic)a * a * a - a) * _y2[klo] + (b * (dynamic)b * b - b) * _y2[khi]) * (h * (dynamic)h) * 0.166666666666666666666667;
		}
		#region IInterpolator<Tx,Ty> Members

		public void FeedData(IList<Tx> xs, IList<Ty> ys)
		{
			XData.Clear();
			XData.AddRange(xs);
			YData.Clear();
			YData.AddRange(ys);
			_initiate();
		}

		#endregion

		#region IFunction<Tx,Ty> Members

		public Func<Tx, Ty> Functor
		{
			get { return this.Evaluate; }
		}

		public FunctionAnalysis.IFunction<Tx, Ty> Derivative
		{
			get { throw new NotImplementedException(); }
		}

		public FunctionAnalysis.IFunction<Tx, Ty> Integral
		{
			get { throw new NotImplementedException(); }
		}

		#endregion
	}
}
